


#include "glodef.h"
#include "errlog.h"
#include "vecs.h"
#include "fe.h"
#include "f3io.h"
#include "glovar.h"

#include "soah_common.h"

#include <stdlib.h>

/* these are in soah_periodic.c */
extern int __solve_period(vector u_init, scalar p_init, int period,
    double *norm_u, double *norm_p)__attribute__((warn_unused_result));

extern int __solve_shape_adjoint_period(vector v_finl, scalar q_finl, int period,
    double *norm_v, double *norm_q)__attribute__((warn_unused_result));

extern void erase_backup(const char *dir);

int solve_state_periodic_broyden(vector u_init, scalar p_init)
{
    int ret = TRUE;
    int MAX_N=min(50, MAX_PERIODS);
    int i, period, uN, pN;
    int n, j;
    double norm_u, norm_p, norm_init, *ns, coef, norm_old, norm;

    vector *su, zu;
    scalar *sp, zp;

    PrintLog("Solving periodic state problem by broyden.\n");

    uN=get_N_vec(u_init);
    pN=get_N_scal(p_init);

    period=1;
    ret=__solve_period(u_init, p_init, period, &norm_u, &norm_p);
        if(!ret) return ret;
    norm=norm_init=fmax(norm_u, norm_p);
    n=-1;

    if(norm_init<PERIODIC_TOL)
    {
        erase_backup(state_dir());
        return ret;
    }

    su=malloc(MAX_N*sizeof(vector));
    sp=malloc(MAX_N*sizeof(scalar));
    ns=malloc(MAX_N*sizeof(double));

    if(su==NULL||sp==NULL) error_msg("%s : cannot allocate memory.\n", __func__);
    for(i=0; i<MAX_N; i++)
    {
        su[i]=alloc_vec(uN);
        sp[i]=alloc_scal(pN);
    }
    zu=alloc_vec(uN);
    zp=alloc_scal(pN);

    // for(i=0; i<3*uN; i++) su[0].X[i] = u->X[i] - u_init->X[i];
    // for(i=0; i<pN; i++)  sp[0].F[i] = p->F[i] - p_init->F[i];
    waxpy_vec(su[0], -1.0, u_init, u);
    waxpy_scal(sp[0], -1.0, p_init, p);

    // ns[0] = 0.0;
    // for(i=0;i<3*uN; i++) ns[0] += sqr(su[0].X[i]);
    // for(i=0;i<pN; i++) ns[0] += sqr(sp[0].F[i]);    
    ns[0]=dot_vec(su[0], su[0]);
    ns[0]+=dot_scal(sp[0], sp[0]);

    for(period++; period<=MAX_PERIODS; period++)
    {
        n++;

        // for(i=0; i<3*uN; i++) u_init->X[i] += su[n].X[i];
        // for(i=0; i<pN; i++) p_init->F[i] += sp[n].F[i];
        axpy_vec(u_init, 1.0, su[n]);
        axpy_scal(p_init, 1.0, sp[n]);
        ret=__solve_period(u_init, p_init, period, &norm_u, &norm_p);
        if(!ret)
            break;
        norm_old=norm;
        norm=fmax(norm_u, norm_p);
        if(norm<1e-6*norm_init+PERIODIC_TOL)
            break;

        if(n+1<MAX_N)
        {
            // for(i=0; i<3*uN; i++) zu.X[i] = u->X[i] - u_init->X[i];
            // for(i=0; i<pN; i++)   zp.F[i] = p->F[i] - p_init->F[i];
            waxpy_vec(zu, -1.0, u_init, u);
            waxpy_scal(zp, -1.0, p_init, p);

            for(j=0; j<n; j++)
            {
                // coef = 0.0;
                // for(i=0; i<3*uN; i++) coef += zu.X[i] * su[j].X[i];
                // for(i=0; i<pN; i++) coef += zp.F[i] * sp[j].F[i];
                coef=dot_vec(zu, su[j]);
                coef+=dot_scal(zp, sp[j]);
                coef/=ns[j];

                // for(i=0; i<3*uN; i++) zu.X[i] += su[j+1].X[i] * coef;
                // for(i=0; i<pN; i++)   zp.F[i] += sp[j+1].F[i] * coef;
                axpy_vec(zu, coef, su[j+1]);
                axpy_scal(zp, coef, sp[j+1]);
            }

            // coef = 0.0;
            // for(i=0; i<3*uN; i++) coef += zu.X[i] * su[n].X[i];
            // for(i=0; i<pN; i++) coef += zp.F[i] * sp[n].F[i];
            coef=dot_vec(zu, su[n]);
            coef+=dot_scal(zp, sp[n]);
            coef=1.0/(1.0-coef/ns[n]);

            // for(i=0; i<3*uN; i++) su[n+1].X[i] = zu.X[i] * coef;
            // for(i=0; i<pN; i++) sp[n+1].F[i] = zp.F[i] * coef;
            clean_vec(su[n+1]);
            axpy_vec(su[n+1], coef, zu);
            clean_scal(sp[n+1]);
            axpy_scal(sp[n+1], coef, zp);

            // ns[n+1] = 0;
            // for(i=0;i<3*uN; i++) ns[n+1] += sqr(su[n+1].X[i]);
            // for(i=0;i<pN; i++) ns[n+1] += sqr(sp[n+1].F[i]);
            ns[n+1]=dot_vec(su[n+1], su[n+1]);
            ns[n+1]+=dot_scal(sp[n+1], sp[n+1]);
        }
        else
        {
            n= -1;
            // for(i=0; i<3*uN; i++) su[0].X[i] = u->X[i] - u_init->X[i];
            // for(i=0; i<pN; i++)   sp[0].F[i] = p->F[i] - p_init->F[i];
            waxpy_vec(su[0], -1.0, u_init, u);
            waxpy_scal(sp[0], -1.0, p_init, p);
        }

    }

    erase_backup(state_dir());
    for(i=0; i<MAX_N; i++)
    {
        free_vec(su[i]);
        free_scal(sp[i]);
    }
    free_vec(zu);
    free_scal(zp);
    free(su);
    free(sp);
    free(ns);

    return ret;
}

int solve_adjoint_shape_periodic_broyden(vector v_finl, scalar q_finl)
{
    int ret=TRUE;
    int MAX_N=min(50, MAX_PERIODS);
    int i, period, vN, qN;
    int n, j;
    double norm_v, norm_q, norm_init, *ns, coef, norm_old, norm;

    vector *sv, zv;
    scalar *sq, zq;

    vN=get_N_vec(v_finl);
    qN=get_N_scal(q_finl);

    PrintLog("Solving periodic adjoint problem by broyden.\n");


    period=1;
    ret=__solve_shape_adjoint_period(v_finl, q_finl, period, &norm_v, &norm_q);
    if(!ret)
        return ret;
    norm=norm_init=fmax(norm_v, norm_q);
    n=-1;

    if(norm_init<PERIODIC_TOL)
    {
        erase_backup(adjoint_dir());
        return ret;
    }

    sv=malloc(MAX_N*sizeof(vector));
    sq=malloc(MAX_N*sizeof(scalar));
    ns=malloc(MAX_N*sizeof(double));

    if(sv==NULL||sq==NULL||ns==NULL)
        error_msg("%s : cannot allocate memory.\n", __func__);
    for(i=0; i<MAX_N; i++)
    {
        sv[i]=alloc_vec(vN);
        sq[i]=alloc_scal(qN);
    }
    zv=alloc_vec(vN);
    zq=alloc_scal(qN);

    // for(i=0; i<3*vN; i++) sv[0].X[i] = v->X[i] - v_finl->X[i];
    // for(i=0; i<qN; i++)  sq[0].F[i] = q->F[i] - q_finl->F[i];
    waxpy_vec(sv[0], -1.0, v_finl, v);
    waxpy_scal(sq[0], -1.0, q_finl, q);

    // ns[0] = 0.0;
    // for(i=0;i<3*vN; i++) ns[0] += sqr(sv[0].X[i]);
    // for(i=0;i<qN; i++) ns[0] += sqr(sq[0].F[i]);
    ns[0]=dot_vec(sv[0], sv[0]);
    ns[0]+=dot_scal(sq[0], sq[0]);

    for(period++; period<=MAX_PERIODS; period++)
    {
        n++;
        // for(i=0; i<3*vN; i++) v_finl->X[i] += sv[n].X[i];
        // for(i=0; i<qN; i++) q_finl->F[i] += sq[n].F[i];
        axpy_vec(v_finl, 1.0, sv[n]);
        axpy_scal(q_finl, 1.0, sq[n]);

        ret=__solve_shape_adjoint_period(v_finl, q_finl, period, &norm_v, &norm_q);
        if(!ret)
            break;
        norm_old=norm;
        norm=fmax(norm_v, norm_q);
        if(norm<1e-6*norm_init+PERIODIC_TOL)
            break;

        if(n+1<MAX_N)
        {
            // for(i=0; i<3*vN; i++) zv.X[i] = v->X[i] - v_finl->X[i];
            // for(i=0; i<qN; i++)   zq.F[i] = q->F[i] - q_finl->F[i];
            waxpy_vec(zv, -1.0, v_finl, v);
            waxpy_scal(zq, -1.0, q_finl, q);

            for(j=0; j<n; j++)
            {
                // coef = 0.0;
                // for(i=0; i<3*vN; i++) coef += zv.X[i] * sv[j].X[i];
                // for(i=0; i<qN; i++) coef += zq.F[i] * sq[j].F[i];
                coef=dot_vec(zv, sv[j]);
                coef+=dot_scal(zq, sq[j]);
                coef/=ns[j];

                // for(i=0; i<3*vN; i++) zv.X[i] += sv[j+1].X[i] * coef;
                // for(i=0; i<qN; i++)   zq.F[i] += sq[j+1].F[i] * coef;
                axpy_vec(zv, coef, sv[j+1]);
                axpy_scal(zq, coef, sq[j+1]);
            }

            // coef = 0.0;
            // for(i=0; i<3*vN; i++) coef += zv.X[i] * sv[n].X[i];
            // for(i=0; i<qN; i++) coef += zq.F[i] * sq[n].F[i];
            coef=dot_vec(zv, sv[n]);
            coef+=dot_scal(zq, sq[n]);
            coef=1.0/(1.0-coef/ns[n]);

            // for(i=0; i<3*vN; i++) sv[n+1].X[i] = zv.X[i] * coef;
            // for(i=0; i<qN; i++) sq[n+1].F[i] = zq.F[i] * coef;
            clean_vec(sv[n+1]);
            axpy_vec(sv[n+1], coef, zv);
            clean_scal(sq[n+1]);
            axpy_scal(sq[n+1], coef, zq);

            // ns[n+1] = 0;
            // for(i=0;i<3*vN; i++) ns[n+1] += sqr(sv[n+1].X[i]);
            // for(i=0;i<  qN; i++) ns[n+1] += sqr(sq[n+1].F[i]);
            ns[n+1]=dot_vec(sv[n+1], sv[n+1]);
            ns[n+1]+=dot_scal(sq[n+1], sq[n+1]);
        }
        else
        {
            n= -1;
            // for(i=0; i<3*vN; i++) sv[0].X[i] = v->X[i] - v_finl->X[i];
            // for(i=0; i<  qN; i++) sq[0].F[i] = q->F[i] - q_finl->F[i];
            waxpy_vec(sv[0], -1.0, v_finl, v);
            waxpy_scal(sq[0], -1.0, q_finl, q);
        }

    }

    erase_backup(adjoint_dir());
    for(i=0; i<MAX_N; i++)
    {
        free_vec(sv[i]);
        free_scal(sq[i]);
    }
    free_vec(zv);
    free_scal(zq);
    free(sv);
    free(sq);
    free(ns);

    return ret;
}

